//------------------------------------------------------------------------------
//  <copyright file="ManagedKukaFriThunk.h" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
//  </copyright>
//------------------------------------------------------------------------------
#pragma once
#include "IManagedFri.h"
#include "FRIdispatcher.h"

using namespace System;
using namespace System::Collections::Generic;
using namespace System::Runtime::InteropServices;

namespace Microsoft 
{
    namespace Robotics
    {
        namespace Hardware
        {
            namespace KukaLBR
            {
                /// Thunk layer that translates calls from the unmanaged Kuka Dispatcher FRI to a user supplied FRI
                public ref class ManagedKukaFriThunk sealed : IManagedFRI, IDisposable
                {
                public:
                    /// <summary> Creates a ManagedKukaFriThunk as the thunk layer to a managed FRI. </summary>
                    /// <param name="managedFRIConnection"> Managed FRI handler </param>
                    ManagedKukaFriThunk(IManagedFRI^ managedApi);

                    /// <summary>  ManagedKukaFriThunk Destructor - releases unmanaged resources </summary>
                    ~ManagedKukaFriThunk();

                    /// <summary>  get the underlying unmanged FRI connection </summary>
                    IFRIConnection * GetFriConnection() {return this->pFRIconnection;}

                    // Interop to managed Api callbacks - implements IManagedFRI

                    /// <returns> Status code.  0 on no error. </returns>
                    virtual int InitializeFRI();

                    /// <summary> Shut down the FRI server. </summary>
                    /// <returns> Status code.  0 on no error. </returns>
                    virtual int CloseFRI();

                    /// <summary> Wait for an FRI client to connect. </summary>
                    /// <returns> Status code.  0 on no error. </returns>
                    virtual int WaitOnFRIConnect();

                    /// <summary> Wait on a new FRI request. </summary>
                    /// <returns> State of the LBR as indicated by the next FRI request. </returns>
                    virtual int GetFRIRequest(double **ppFeedbackRegisters);

                    /// <summary> Send a response to the FRI request, but take no control actions in it. </summary>
                    /// <returns> Status code.  0 on no error. </returns>
                    virtual int DoNothing();

                    /// <summary> Execute joint position control. </summary>
                    /// <param name="jointPosition"> Array of joint positions. </param>
                    /// <returns> Status code.  0 on no error. </returns>
                    virtual int DoPositionControl(double *jointPosition);

                    /// <summary> Execute joint impedance control. </summary>
                    /// <param name="jointPosition"> Array of joint positions. </param>
                    /// <param name="jointStiffness"> Array of joint stiffness values. </param>
                    /// <param name="jointDamping"> Array of joint damping values. </param>
                    /// <param name="jointAdditionalTorque"> Array of addition joint torque values. </param>
                    /// <returns> Status code.  0 on no error. </returns>
                    virtual int DoJointImpedanceControl(
                        double *jointPosition,
                        double *jointStiffness,
                        double *jointDamping,
                        double *jointAdditionalTorque);

                    /// <summary> Execute Cartesian impedance control. </summary>
                    /// <param name="cartesianPosition"> Array indicating desired cartesian position relative to base frame. </param>
                    /// <param name="cartesianStiffness"> Array of Cartesian stiffness values. </param>
                    /// <param name="cartesianDamping"> Array of Cartesian damping values. </param>
                    /// <param name="additionalTCPForceTorque"> Array of additional TCP force torque values. </param>
                    virtual int DoCartesianImpedanceControl(
                        double *cartesianPosition,
                        double *cartesianStiffness,
                        double *cartesianDamping,
                        double *additionalTCPForceTorque);

					// <summary> Execute Cartesian impedance control using KRL motions. </summary>
					/// <param name="cartesianPosition"> Array indicating desired cartesian position relative to base frame. </param>
					/// <param name="sequenceNumber"> Sequence number of the command. </param>
					virtual int DoKrlCartesianImpedanceControl(double *cartesianPosition, int sequenceNumber);

				private:
					// Wire up callbacks to the managed FRI
					void SetDispatcherHandlers(IManagedFRI^ managedApi);

					//delegate types
                    delegate int Parameter0Delegate();
                    delegate int ParameterPD1Delegate(double *parm1);
                    delegate int ParameterPPD1Delegate(double **parm1);
                    delegate int ParameterPD4Delegate(double *parm1, double *parm2, double *parm3, double *parm4);
					delegate int ParameterPD1I1Delegate(double *parm1, int parm2);

                    /// <summary> Create a unmanaged call back with 0 parameter </summary>
                    /// <param name="managedApi">delegate for callback</param>
                    FRICALLBACK0 CreateCallBack0(Parameter0Delegate ^delgate0);

                    /// <summary> Create a unmanaged call back with 1 parameter </summary>
                    /// <param name="managedApi">delegate for callback</param>
                    FRICALLBACKPD1 CreateCallBackPD1(ParameterPD1Delegate ^delgate1);

                    /// <summary> Create a unmanaged call back with 1 array pointer parameter</summary>
                    /// <param name="managedApi">delegate for callback</param>
                    FRICALLBACKPPD1 CreateCallBackPPD1(ParameterPPD1Delegate ^delgate1);

                    /// <summary> Create a unmanaged call back with 4 parameter </summary>
                    /// <param name="managedApi">delegate for callback</param>
                    FRICALLBACKPD4 CreateCallBackPD4(ParameterPD4Delegate ^delgate4);

                    /// <summary> Create a unmanaged call back with 1 double * and 1 int parameter </summary>
                    /// <param name="managedApi">delegate for callback</param>
					FRICALLBACKPD1I1 CreateCallBackPD1I1(ParameterPD1I1Delegate ^delgatePD1I1);

                    // Simply used to stop garbagae collection of the callbacks
                    List<GCHandle> ^delegateHandles;

                    // Connection the the unmanaged dispatcher
                    IFRIConnection *pFRIconnection;

                    // Thunk layer to user supplied managed FRI
                    IManagedFRI^ managedFRIConnection;
                };
            }
        }
    }
}
